/**
 * @file GridMap.h
 * @author rj.wang (rj.wang@aotbot.com)
 * @brief 栅格地图基类
 * @version 0.1
 * @date 2024-11-04
 * @copyright Copyright (c) 2024
 */

#ifndef __COMMON_PLANNING_GRID_MAP_H_
#define __COMMON_PLANNING_GRID_MAP_H_

#include <stdint.h>
#include <sstream>
#include <string>

#include "Pose2D.h"

namespace common {
namespace planning {

// 1. GridMap
// 1.1 coordinate value in world-coordinate of left-bottom-grid's center
// 1.2 grid resolution
// 1.3 height & width
// 1.4 data (vector)
// 1.5 grid state (T)

template <class T>
class GridMap {
protected:
  //x coordinate of left-bottom grid's center in world
  double origin_center_x_{0.0}; //unit: m
  //y coordinate of left-bottom grid's center in world
  double origin_center_y_{0.0}; //unit: m
  //grid's side length
  double resolution_{0.05}; //unit: m
  //colomns' size
  int64_t width_{0};
  //rows' size
  int64_t height_{0};
  //map's data
  std::vector<T> data_{};
  
public:
  //interfaces
  //initialize data_, width_, height_
  inline bool SetMapData(
      const std::vector<T>& map, 
      const int64_t width, 
      const int64_t height) {
    if(map.size() != size_t(width * height)) {
      return false;
    } else if(map.empty()) {
      return false;
    }
    data_ = map;
    width_ = width;
    height_ = height;
    return true;
  }

  //set origin_center_x_, origin_center_y_
  inline void SetOrigin(const Pose& origin) {
    origin_center_x_ = origin.X();
    origin_center_y_ = origin.Y();
  }

  //set width_
  inline void SetWidth(const int64_t width) {
    width_ = width;
  }

  //set height_
  inline void SetHeight(const int64_t height) {
    height_ = height;
  }

  //set resolution_
  inline void SetResolution(const double resolution) {
    resolution_ = resolution;
  }

  //get reference of grid map's data_
  inline std::vector<T>& MapData() { return data_; }

  //get const reference of grid map's data_
  inline const std::vector<T>& GetMapData() const { return data_; }

  //get width_
  inline int64_t Width() const { return width_; }

  //get height_
  inline int64_t Height() const { return height_; }

  //get resolution_
  inline double Resolution() const { return resolution_; }

  //get origin_center_x_, origin_center_y_
  inline Pose Origin() const { 
    return Pose(origin_center_x_, origin_center_y_, 0); 
  }

  //xmin of gridmap in world coordinate
  virtual inline double GetXmin() const {
    return origin_center_x_ - 0.5 * resolution_;
  }
  
  //xmax of gridmap in world coordinate
  virtual inline double GetXmax() const {
    return GetXmin() + width_ * resolution_;
  }
  
  //ymin of gridmap in world coordinate
  virtual inline double GetYmin() const {
    return origin_center_y_ - 0.5 * resolution_;
  }
  
  //ymax of gridmap in world coordinate
  virtual inline double GetYmax() const {
    return GetYmin() + height_ * resolution_;
  }
  
  //does grid(ix, iy) belong to this grid map
  virtual inline bool IsValid(const int64_t ix, const int64_t iy) const {
    return 0 <= ix && ix < width_ && 0 <= iy && iy < height_;
  }
  
  //transform from world coordinate x to grid map ix
  virtual inline int64_t X2IX(const double x) const {
    return round((x - origin_center_x_) / resolution_);
  }
  
  //transform from world coordinate y to grid map iy
  virtual inline int64_t Y2IY(const double y) const {
    return round((y - origin_center_y_) / resolution_);
  }
  
  //transform from grid map ix to world coordinate x
  virtual inline double IX2X(const int64_t ix) const {
    return ix * resolution_ + origin_center_x_;
  }
  
  //transform from grid map iy to world coordinate y
  virtual inline double IY2Y(const int64_t iy) const {
    return iy * resolution_ + origin_center_y_;
  }

  //get grid(ix, iy) state
  virtual inline T GetGrid(const int64_t ix, const int64_t iy) const = 0;
  
  //set grid(ix, iy) state value
  virtual inline void SetGrid(const int64_t ix, const int64_t iy, const T value) = 0;
  
  //print the gridmap
  virtual void PrintMap() const = 0;

};

} // namespace planning
} // namespace common

#endif // __COMMON_PLANNING_GRID_MAP_H_